
int switchPin1 = 4;
int a = 0;  //暂停按键控制反转变量

int switchPin2 = 5;
int flage = 0;
int switchPin3 = 47;
int b = 0;  
int c=0;                    //暂停按键控制反转变量
unsigned int Motor_AIN_A1 = 7;  //控制A电机的PWM引脚
unsigned int Motor_AIN_A2 = 8;
String Target_Value;  //串口获取的速度字符串变量
int value;            //用于存储通过PI控制器计算得到的用于调整电机转速的PWM值的整形变量

unsigned int Motor_AIN_B1 = 44;  //控制B电机的PWM引脚
unsigned int Motor_AIN_B2 = 45;
String Target_Value_1;  //串口获取的速度字符串变量
int value_1;            //用于存储通过PI控制器计算得到的用于调整电机转速的PWM值的整形变量

#include <FlexiTimer2.h>  //定时中断头文件库
/***********编码器引脚************/
#define ENCODER_A 2                                    //编码器A相引脚
#define ENCODER_B 50                                   //编码器B相引脚
int Velocity, Count = 0;                               //Count计数变量 Velocity存储设定时间内A相上升沿和下降沿的个数
float Velocity_KP = 2, Velocity_KI = 0.6, Target = 0;  //Velocity_KP,Velocity_KI.PI参数  Target目标值


int buzzerPin = 41;
int led = 39;
#define ENCODER_C 20                                         //编码器A相引脚
#define ENCODER_D 51                                         //编码器B相引脚
int Velocity_1, Count_1 = 0;                                 //Count计数变量 Velocity存储设定时间内A相上升沿和下降沿的个数
float Velocity_KP_1 = 2, Velocity_KI_1 = 0.6, Target_1 = 0;  //Velocity_KP,Velocity_KI.PI参数  Target目标值

/*********** 限幅************
*以下两个参数让输出的PWM在一个合理区间
*当输出的PWM小于40时电机不转 所以要设置一个启始PWM
*arduino mega 2560 单片机的PWM不能超过255 所以 PWM_Restrict 起到限制上限的作用
*****************************/
int startPWM = 0;          //初始PWM
int PWM_Restrict = 100;    //artPW+PWM_Restric=255<256
int startPWM_1 = 0;        //初始PWM
int PWM_Restrict_1 = 100;  //artPW+PWM_Restric=255<256
static int t = 0, sum = 0;
double y = 0.1761;


int i[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int tl = 0;
int tr = 0;
void qusetion_1();
void tiaozheng();
void setup() {
  Serial.begin(9600);  //打开串口
  Serial.println("/*****开始驱动*****/");

  setBegin();     //电机工作
  set_xunxian();  //灰度传感器串口定义
  pinMode(41, OUTPUT);
   pinMode(39, OUTPUT);
}
int p1 = 1;
int p2 = 1;
int p3 = 1;
int p4 = 1;
int block = 0;
int number = 0;
int g = 0;
int tiao = 0;
int chasu_1=12;
int mmm = 0;
int biao_1 = 0;//标志位
int biao_2 = 0;
int zhuanhuan = 0;
int zhuanhuan_in_1 = 0;
int begin_1=0;
int begin_2=0;
int zhuanhuan_in_2 = 0;
void loop() {

  digitalWrite(buzzerPin, HIGH);  // 设置蜂鸣器引脚为低电平
  digitalWrite(led, LOW);        // 设置蜂鸣器引脚为低电平

  set_xun();  //七路传感器信息读取
 //第一题
  if (digitalRead(switchPin1)) {
    a = !a;
    while (digitalRead(switchPin1));
      delay(500);
      beep();
  }
  if (a == 1) {
    qusetion_1();
    Serial.println(123);  // 打印接收到的数据
  }

  //第二题
  if (digitalRead(switchPin2)) {
    b = !b;
    while (digitalRead(switchPin2));
     delay(500);
     beep();
  }
 
  if (b == 1) {
    //一旦有黑线出现就开始巡线
    if (i[1] != 0 || i[2] != 0 || i[3] != 0 || i[4] != 0 || i[5] != 0 || i[6] != 0 || i[7] != 0) {
      xunxian();
    }
    //
    tiaozheng();

  //全为白则直行--A------B的路径
    if (i[1] == 0 && i[2] == 0 && i[3] == 0 && i[4] == 0 && i[5] == 0 && i[6] == 0 && i[7] == 0) {
     
      motor(60, 59);
    }

//全为白且已经识别过黑线--A
    if (i[1] == 0 && i[2] == 0 && i[3] == 0 && i[4] == 0 && i[5] == 0 && i[6] == 0 && i[7] == 0 && biao_2 == 2) {
      beep();
      while (1) {
        // beep();
        motor(0, 0);
      }
    }
    Serial.println(456);  // 打印接收到的数据
  }
  //第二题
  if (digitalRead(switchPin3)) {
    c = !c;
    while (digitalRead(switchPin3));
     delay(500);
     beep();
  }
  if(c==1)
  {
    question_3();
  }
}
    

void qusetion_1() {
  motor(60, 55);
  if (i[1] != 0 || i[2] != 0 || i[3] != 0 || i[4] != 0 || i[5] != 0 || i[6] != 0 || i[7] != 0) {
    while (1) {
      beep();
      while(1)
      {
        motor(0, 0);
      }
    }
  }
}

void motor(int left, int right) {
  Target = map(left, 0, 255, 0, 20);      //右轮转速，正为前
  Target_1 = -map(right, 0, 255, 0, 20);  //左轮转速，正为反
}

void set_xunxian() {
  pinMode(22, INPUT);
  pinMode(24, INPUT);
  pinMode(26, INPUT);
  pinMode(28, INPUT);
  pinMode(30, INPUT);
  pinMode(32, INPUT);
  pinMode(34, INPUT);
}



void set_xun() {


  i[7] = digitalRead(22);  //检测到黑线为高电平（1），白线为低电平（0)
  i[6] = digitalRead(24);
  i[5] = digitalRead(26);
  i[4] = digitalRead(28);
  i[3] = digitalRead(30);
  i[2] = digitalRead(32);
  i[1] = digitalRead(34);
}



void tiaozheng() {
//只要有黑线
  if (i[1] != 0 || i[2] != 0 || i[3] != 0 || i[4] != 0 || i[5] != 0 || i[6] != 0 || i[7] != 0) {
    zhuanhuan++;
    Serial.print("输出zhuanhuan");
    Serial.println(zhuanhuan);
//每50次改变一次标志位
    if (zhuanhuan == 40) {
      biao_1 = 1;
      biao_2 += 1;
    }
    //B点和C点，通过串口查看zhuanhan的值确定其区间
    if (zhuanhuan == 1 || zhuanhuan >=123&&zhuanhuan<=126) {
      beep();
    }
    //D点
    if (zhuanhuan >= 127 && zhuanhuan <= 130) {
      beep();
    }
  }
//每180归零一次
  if (zhuanhuan == 180) {
    zhuanhuan = 2;
  }
  int n = 100;
  //全为白
  if (i[1] == 0 && i[2] == 0 && i[3] == 0 && i[4] == 0 && i[5] == 0 && i[6] == 0 && i[7] == 0) {
    // Serial.print("前输出");
    Serial.println(mmm);
    mmm++;
    //110次全白且已经识别过黑线--C------D校准
    if (mmm >= 110 && biao_1 == 1) {

      motor(120, 35);
      n--;
      Serial.print(n);
      Serial.print("加速实现调整");
      if (n = 1) {
        mmm = 0;
        n == 100;
      }
    }
  }
}

//第三问
int yuandi = 0;
int yuandi_1 = 0;
int zhi = 0;
int zhi_1 = 0;
int zhuanwan = 0;
int zhuanwan_1 = 0;
int back_yuandi = 0;
int back_zhi = 0;
int back_zhuanwan = 0;

void question_3() {
  if (begin_1 == 0&&begin_2==0) {
    for (; yuandi <= 36; yuandi++) { // 开始时转弯的时间
      motor(60, 0); // 开始时转弯的速度
      Serial.print("yuandi ");
      Serial.println(yuandi);
    }
    for (; zhi <= 310; zhi++) { // 开始时直行的时间
      motor(100, 100); // 开始时直线的速度
      Serial.print("zhi ");
      Serial.println(zhi);
    }
    for (; zhuanwan <= 55; zhuanwan++) { // 到达时左转的时间
      motor(0, 35); // 到达时左转的速度
      delay(15);
      motor(20, 0); // 适当延时后调整速度
      Serial.print("zhuanwan ");
      Serial.println(zhuanwan);
    }
    motor(0, 0); // 到达 C 点时停止
    beep();
    delay(500);
    motor(22, 20);  
    begin_1 = 1; // 将标志位置一,上C--B线
    Serial.println(begin_1);

  }
  xunxian_left (); // 左转的巡线

  if (begin_1 == 1&&begin_2==0) {
    //只要识别到黑线
    if (i[1] != 0 || i[2] != 0 || i[3] != 0 || i[4] != 0 || i[5] != 0 || i[6] != 0 || i[7] != 0) {
      zhuanhuan_in_1++; // 黑线标志位

      if (zhuanhuan_in_1 == 41) {
        zhuanhuan_in_1 = 40;
      }

      Serial.print("zhuanhuan_in_1_");    
      Serial.println(zhuanhuan_in_1);
    }
    //全白
    if (i[1] == 0 && i[2] == 0 && i[3] == 0 && i[4] == 0 && i[5] == 0 && i[6] == 0 && i[7] == 0 && zhuanhuan_in_1 >= 40) {
      zhuanhuan_in_2++; // 白线标志位
      Serial.print("zhuanhuan_in_2_");
      Serial.println(zhuanhuan_in_2);
    //B点
      if (zhuanhuan_in_1 == 40 && zhuanhuan_in_2 >= 28) { // 黑白交接的动作设置
        Serial.println("执行转弯和直行操作");
        begin_1=0;
        begin_2=1;
        beep();
        motor(0, 0);
        delay(2000);

        // 左转第二阶段--B--D
        if (begin_2 ==1&&begin_1==0) {
          for (; yuandi_1 <= 36; yuandi_1++) { // 开始时转弯的时间
            motor(0, 50); // 开始时转弯的速度
            Serial.print("yuandi_1 ");
            Serial.println(yuandi_1);
          }

          while (1) {
            motor(100, 100); // 开始时直行的速度
            delay(2900);
            break;
          }

          for (; zhuanwan_1 <= 55; zhuanwan_1++) { // 到达时左转的时间
            motor(40, 0); // 到达时左转的速度
            delay(15);
            motor(0, 20); // 适当延时后调整速度
            Serial.print("zhuanwan_1 ");
            Serial.println(zhuanwan_1);
          }
          beep();
          motor(0, 0); // 到达 D点时停止
          delay(500);
          motor(20, 22);
          begin_1 = 1; // 将标志位置一
          Serial.println(begin_2);
        }

        //xunxian(); // 左转的巡线
    
      }
      //xunxian();
    }
    //xunxian();
  }
  if(begin_1 == 1&& begin_2==1) 
  {
    xunxian();
    flage=flage+ 1;
    if (i[1] == 0 && i[2] == 0 && i[3] == 0 && i[4] == 0 && i[5] == 0 && i[6] == 0 && i[7] == 0&&flage>=20 )
      {
          beep();
          motor(0, 0); // 到达 D点时停止
          delay(500);
      }
  }
  
//xunxian();
}

void xunxian_left() {
  if (i[3] == 0 && i[4] != 0 && i[5] == 0)
  {
    motor(60, 60 + chasu_1);
    Serial.println("居中");
  }
  else if (i[3] != 0 && i[4] != 0 && i[5] != 0)
  {
    motor(60, 60 + chasu_1);
    Serial.println("居中");
  }
  else if (i[3] != 0 && i[4] != 0 && i[5] == 0)
  {
    motor(60, 65 + chasu_1);
    Serial.println("右微微偏");
  }
  else if (i[3] == 0 && i[4] != 0 && i[5] != 0)
  {
    motor(65, 60 + chasu_1);
    Serial.println("左微微偏");
  }
  else if (i[3] != 0 && i[4] == 0 && i[5] == 0)
  {
    motor(60, 70 + chasu_1);
    Serial.println("右微偏");
  }
  else if (i[3] == 0 && i[4] == 0 && i[5] != 0)
  {
    motor(70, 60 + chasu_1);
    Serial.println("左微偏");
  }
  else if ( i[2] != 0 && i[4] == 0)
  {
    motor(60, 75 + chasu_1);
    Serial.println("右偏");
  }
  else if ( i[6] != 0 && i[4] == 0)
  {
    motor(75, 60 + chasu_1);
    Serial.println("左偏");
  }

  else if ( i[7] != 0 && i[4] == 0)
  {

    motor(80, 60 + chasu_1);
    Serial.println("左偏过头");

  }
  else if ( i[1] != 0 && i[4] == 0)
  {
    motor(60, 80 + chasu_1);
    Serial.println("右偏过头");
  }
}
void xunxian_right() {
  if (i[3] == 0 && i[4] != 0 && i[5] == 0)
  {
    motor(60, 60 + chasu_1);
    Serial.println("居中");
  }
  else if (i[3] != 0 && i[4] != 0 && i[5] != 0)
  {
    motor(60, 60 + chasu_1);
    Serial.println("居中");
  }
  else if (i[3] != 0 && i[4] != 0 && i[5] == 0)
  {
    motor(60, 65 + chasu_1);
    Serial.println("右微微偏");
  }
  else if (i[3] == 0 && i[4] != 0 && i[5] != 0)
  {
    motor(65, 60 + chasu_1);
    Serial.println("左微微偏");
  }
  else if (i[3] != 0 && i[4] == 0 && i[5] == 0)
  {
    motor(60, 70 + chasu_1);
    Serial.println("右微偏");
  }
  else if (i[3] == 0 && i[4] == 0 && i[5] != 0)
  {
    motor(70, 60 + chasu_1);
    Serial.println("左微偏");
  }
  else if ( i[2] != 0 && i[4] == 0)
  {
    motor(60, 75 + chasu_1);
    Serial.println("右偏");
  }
  else if ( i[6] != 0 && i[4] == 0)
  {
    motor(75, 60 + chasu_1);
    Serial.println("左偏");
  }

  else if ( i[7] != 0 && i[4] == 0)
  {

    motor(80, 60 + chasu_1);
    Serial.println("左偏过头");

  }
  else if ( i[1] != 0 && i[4] == 0)
  {
    motor(60, 80 + chasu_1);
    Serial.println("右偏过头");
  }
}

void beep() {

  digitalWrite(buzzerPin, LOW);  // 设置蜂鸣器引脚为高电平
  digitalWrite(led, HIGH);        // 设置蜂鸣器引脚为高电平
  delay(20);
  digitalWrite(buzzerPin, HIGH);  // 设置蜂鸣器引脚为低电平
  digitalWrite(led, LOW);        // 设置蜂鸣器引脚为低电平
}

int chasu = 15;
void xunxian() {
  if (i[3] == 0 && i[4] != 0 && i[5] == 0) {
    motor(60 + chasu, 60);
    Serial.println("居中");
  } else if (i[3] != 0 && i[4] != 0 && i[5] != 0) {
    motor(60 + chasu, 60);
    Serial.println("居中");
  } else if (i[3] != 0 && i[4] != 0 && i[5] == 0) {
    motor(60 + chasu, 65);
    Serial.println("右微微偏");
  } else if (i[3] == 0 && i[4] != 0 && i[5] != 0) {
    motor(65 + chasu, 60);
    Serial.println("左微微偏");
  } else if (i[3] != 0 && i[4] == 0 && i[5] == 0) {
    motor(60 + chasu, 70);
    Serial.println("右微偏");
  } else if (i[3] == 0 && i[4] == 0 && i[5] != 0) {
    motor(70 + chasu, 60);
    Serial.println("左微偏");
  } else if (i[2] != 0 && i[4] == 0) {
    motor(60 + chasu, 75);
    Serial.println("右偏");
  } else if (i[6] != 0 && i[4] == 0) {
    motor(75 + chasu, 60);
    Serial.println("左偏");
  }

  else if (i[7] != 0 && i[4] == 0) {

    motor(80 + chasu, 60);
    Serial.println("左偏过头");

  } else if (i[1] != 0 && i[4] == 0) {
    motor(60 + chasu, 80);
    Serial.println("右偏过头");
  }
}



/***********************************************************************/
void setBegin()  //电机工作
{
  pinMode(ENCODER_A, INPUT);  //设置两个相线为输入模式
  pinMode(ENCODER_B, INPUT);
  pinMode(Motor_AIN_A1, OUTPUT);  //设置两个驱动引脚为输出模式
  pinMode(Motor_AIN_A2, OUTPUT);

  pinMode(ENCODER_C, INPUT);  //设置两个相线为输入模式
  pinMode(ENCODER_D, INPUT);
  pinMode(Motor_AIN_B1, OUTPUT);  //设置两个驱动引脚为输出模式
  pinMode(Motor_AIN_B2, OUTPUT);

  setPwmFrequency(6, 140);
  setPwmFrequency(45, 140);

  FlexiTimer2::set(5, control);  //5毫秒定时中断函数
  FlexiTimer2::start();          //中断使能
  attachInterrupt(3, READ_ENCODER_C, CHANGE);
  attachInterrupt(0, READ_ENCODER_A, CHANGE);  //开启对应2号引脚的0号外部中断,触发方式为CHANGE 即上升沿和下降沿都触发,触发的中断函数为 READ_ENCODER_A
  Target = 0;                                  //右轮转速，正为前
  Target_1 = 0;                                //左轮转速，正为反
}

//Mega 2560 PWM调频率（通过调节精度来实现）1/8分频
void setPwmFrequency(int pin, int precision) {
  if (pin == 11 || pin == 12 || pin == 13) {  //timer 1(16位)
    TCCR1A = _BV(COM1A0) | _BV(COM1B1) | _BV(WGM11) | _BV(WGM10);
    TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);
    OCR1A = precision;
  } else if (pin == 10 || pin == 9) {  //timer 2(8位)
    TCCR2A = _BV(COM2A0) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
    TCCR2B = _BV(WGM22) | _BV(CS21);
    OCR2A = precision;
  } else if (pin == 5 || pin == 2 || pin == 3) {  //timer 3(16位)
    TCCR3A = _BV(COM3A0) | _BV(COM3B1) | _BV(WGM31) | _BV(WGM30);
    TCCR3B = _BV(WGM33) | _BV(WGM32) | _BV(CS31);
    OCR3A = precision;
  } else if (pin == 6 || pin == 7 || pin == 8) {  //timer 4(16位)
    TCCR4A = _BV(COM4A0) | _BV(COM4B1) | _BV(WGM41) | _BV(WGM40);
    TCCR4B = _BV(WGM43) | _BV(WGM42) | _BV(CS41);
    OCR4A = precision;
  } else if (pin == 46 || pin == 45 || pin == 44) {  //timer 5(16位)
    TCCR5A = _BV(COM5A0) | _BV(COM5B1) | _BV(WGM51) | _BV(WGM50);
    TCCR5B = _BV(WGM53) | _BV(WGM52) | _BV(CS51);
    OCR5A = precision;
  }
}
void READ_ENCODER_A() {

  if (digitalRead(ENCODER_A) == HIGH) {
    if (digitalRead(ENCODER_B) == LOW)
      Count--;  //根据另外一相电平判定方向
    else
      Count++;
  }

  else {
    if (digitalRead(ENCODER_B) == LOW)
      Count++;  //根据另外一相电平判定方向
    else
      Count--;
  }
  tl++;
}

void READ_ENCODER_C() {

  if (digitalRead(ENCODER_C) == HIGH) {
    if (digitalRead(ENCODER_D) == LOW)
      Count_1--;  //根据另外一相电平判定方向
    else
      Count_1++;
  }

  else {
    if (digitalRead(ENCODER_D) == LOW)
      Count_1++;  //根据另外一相电平判定方向
    else
      Count_1--;
  }
  tr++;
}

/**********定时器中断触发函数*********/
void control() {

  Velocity = Count;                            //把采用周期(内部定时中断周期)所累计的脉冲上升沿和下降沿的个数,赋值给速度
  Count = 0;                                   //并清零
  value = Incremental_PI_A(Velocity, Target);  //通过目标值和当前值在这个函数下算出我们需要调整用的PWM值

  Velocity_1 = Count_1;                                //把采用周期(内部定时中断周期)所累计的脉冲上升沿和下降沿的个数,赋值给速度
  Count_1 = 0;                                         //并清零
  value_1 = Incremental_PI_A_1(Velocity_1, Target_1);  //通过目标值和当前值在这个函数下算出我们需要调整用的PWM值
  Set_PWM_1(value_1);                                  //将算好的值输出给电机
  Set_PWM(value);                                      //将算好的值输出给电机
}


/***********PI控制器****************/
int Incremental_PI_A(int Encoder, int Target) {
  static float Bias, PWM, Last_bias;                             //定义全局静态浮点型变量 PWM,Bias(本次偏差),Last_bias(上次偏差)
  Bias = Encoder - Target;                                       //计算偏差,目标值减去当前值
  PWM += Velocity_KP * (Bias - Last_bias) + Velocity_KI * Bias;  //增量式PI控制计算

  if (PWM > PWM_Restrict)
    PWM = PWM_Restrict;  //限幅

  if (PWM < -PWM_Restrict)
    PWM = -PWM_Restrict;  //限幅

  Last_bias = Bias;  //保存上一次偏差
  return PWM;        //增量输出
}

int Incremental_PI_A_1(int Encoder_1, int Target_1)  //增量式pid
{
  static float Bias_1, PWM_1, Last_bias_1;                                   //定义全局静态浮点型变量 PWM,Bias(本次偏差),Last_bias(上次偏差)
  Bias_1 = Target_1 - Encoder_1;                                             //计算偏差,目标值减去当前值
  PWM_1 += Velocity_KP_1 * (Bias_1 - Last_bias_1) + Velocity_KI_1 * Bias_1;  //增量式PI控制计算

  if (PWM_1 > PWM_Restrict_1)
    PWM_1 = PWM_Restrict_1;  //限幅

  if (PWM_1 < -PWM_Restrict_1)
    PWM_1 = -PWM_Restrict_1;  //限幅

  Last_bias_1 = Bias_1;  //保存上一次偏差
  return PWM_1;          //增量输出
}

/**********实际控制函数*********/
void Set_PWM(int motora) {
  if (motora > 0)  //如果算出的PWM为正
  {
    analogWrite(Motor_AIN_A1, 0);  //让PWM在设定正转方向(我们认为的正转方向)正向输出调整
    analogWrite(Motor_AIN_A2, motora + startPWM);

  } else if (motora == 0)  //如果PWM为0停车
  {
    analogWrite(Motor_AIN_A2, 0);
    analogWrite(Motor_AIN_A1, 0);
  } else if (motora < 0)  //如果算出的PWM为负
  {
    analogWrite(Motor_AIN_A2, 0);  //让PWM在设定反转方向反向输出调整
    analogWrite(Motor_AIN_A1, -motora + startPWM);
  }
}

void Set_PWM_1(int motora_1) {
  if (motora_1 > 0)  //如果算出的PWM为正
  {
    analogWrite(Motor_AIN_B1, motora_1 + startPWM_1);  //让PWM在设定正转方向(我们认为的正转方向)正向输出调整
    analogWrite(Motor_AIN_B2, 0);
  } else if (motora_1 == 0)  //如果PWM为0停车
  {
    analogWrite(Motor_AIN_B2, 0);
    analogWrite(Motor_AIN_B1, 0);
  } else if (motora_1 < 0)  //如果算出的PWM为负
  {
    analogWrite(Motor_AIN_B2, -motora_1 + startPWM_1);  //让PWM在设定反转方向反向输出调整
    analogWrite(Motor_AIN_B1, 0);
  }
}